/**
    Copyright 2015 Table Rider
    By: Anders Nylander, Christoffer Lindström, Daniel Westerlund
*/

#include <cstdarg>
#include <stdlib.h>
#include <getopt.h>
#include "ComClient.h"
#include "include/tblrdr_cmds.h"

void* constructMessage(unsigned short Cmd, ...);
void sendMsg(ComClient client, void* msg);
int getsz(void* msg);

int main(int argc, char* argv[])
{
    int silent=0;
    if (argc < 4)
    {
        printf("Use: TableRiderComClient <ip> <port> \"message\"\n");
        return 1;
    }

    ComClient client;
    const char *s = client.init(argv[1], atoi(argv[2]));
    if(silent)printf("%s\n", s);
    //printf("%d\n",client.send_msg(argv[3], strlen(argv[3])));
    //printf("%s\n",client.receive_msg());

    void* msg[16];// = malloc(16*sizeof(void*)); // en fuling
    int i;
    for(i=0; i<16; i++)msg[i] = NULL;
    i=0;
    int option_index = 0, opt;
    static struct option loptions[] =
    {
        {"Ping",            no_argument,        0, 'a'},
        {"GetPos",          no_argument,        0, 'b'},
        {"SetPos",          required_argument,  0, 'c'},
        {"GetAngle",        no_argument,        0, 'd'},
        {"SetAngle",        required_argument,  0, 'e'},
        {"MoveTo",          required_argument,  0, 'f'},
        {"MoveBy",          required_argument,  0, 'g'},
        {"SetDoe",          required_argument,  0, 'h'},
        //{"IsJohn",  no_argument,        0, 'i'},
        {"MoveFwd",         required_argument,  0, 'i'},
        {"TurnTo",          required_argument,  0, 'j'},
        {"TurnBy",          required_argument,  0, 'k'},
        {"GetEnc",          no_argument,        0, 'l'},
        {"GetDist",         required_argument,  0, 'm'},
        {"ReqPos",          required_argument,  0, 'n'},
        {"Verbose",         no_argument,        0, 'v'},
        {"DoNextStep",      no_argument,        0, 'w'},
        {"DoAllSteps",      no_argument,        0, 'x'},
        {"Start",           no_argument,        0, 'y'},
        {"Stop",            no_argument,        0, 'z'},
        {"Update",          no_argument,        0, 'A'},
        {"GetCurrent",      no_argument,        0, 'B'},
        {"GetRealCurrent",  no_argument,        0, 'C'},
        {"ControllerInit",  required_argument,  0, 'D'},
        {"ControllerDone",  no_argument,        0, 'E'},
        {"TestLMotor",      no_argument,        0, 'F'},
        {"TestRMotor",      no_argument,        0, 'G'},

        {NULL, 0, 0, 0}
    };

    while(1)
    {
        opt = getopt_long (argc, argv, "abc:de:f:g:h:ij:k:l:m:n:v", loptions, &option_index);
        if (opt==-1) break;
        switch (opt)
        {
        case 'a':
        {
            if(silent)printf("Sending a ping!\n");
            msg[i++] = constructMessage(TBLRDR_PING);
            break;
        }
        case 'b':
        {
            if(silent)printf("Getting Table Rider Position data...\n");
            msg[i++] = constructMessage(TBLRDR_GETPOS);
            break;
        }
        case 'c':
        {
            int k=0,j=0;
            while(optarg[k] != ',' && optarg[k] != '\0'){k++;}
            char* arg1 = (char*)calloc((k+1),sizeof(char));
            memcpy(arg1, optarg, k++);
            double posx = atof(arg1);
            free(arg1);
            while(optarg[k] != '\0'){ k++; j++;}
            arg1 = (char*)calloc((j+1),sizeof(char));
            //char* b = optarg;
            memcpy(arg1, optarg+k-j, j);
            double posy = atof(arg1);
            free(arg1);
            if(silent)printf("Setting Table Rider position...\n");
            msg[i++] = constructMessage(TBLRDR_SETPOS, posx, posy);
            break;
        }
        case 'd':
        {
            if(silent)printf("Getting Table Rider current angle...\n");
            msg[i++] = constructMessage(TBLRDR_GETANGLE);
            break;
        }
        case 'e':
        {
            double angle = atof(optarg);
            if(angle == -1){
                printf("SetAngle only accepts positive integers\n");
                break;
            }
            if(silent)printf("Setting Table Rider current angle...\n");
            msg[i++] = constructMessage(TBLRDR_SETANGLE, angle);
            break;
        }
        case 'f':
        {
            int k=0,j=0;
            while(optarg[k] != ',' && optarg[k] != '\0'){ k++;}
            char* arg1 = (char*)calloc((k+1),sizeof(char));
            for(int o=0; o<i+1; o++) arg1[o] = 0;
            memcpy(arg1, optarg, k++);
            double posx = atof(arg1);
            free(arg1);
            printf("%f\n", posx);
            while(optarg[k] != '\0'){ k++; j++;}
            arg1 = (char*)calloc((j+1),sizeof(char));
            for(int o=0; o<j+1; o++) arg1[o] = 0;
            memcpy(arg1, optarg+k-j, j);
            double posy = atof(arg1);
            free(arg1);
            printf("%f\n", posy);
            if(silent)printf("Sending desired position...\n");
            msg[i++] = constructMessage(TBLRDR_MOVETO, posx, posy);
            break;
        }
        case 'g':
        {
            int k=0,j=0;
            while(optarg[k] != ',' && optarg[k] != '\0'){ k++;}
            char* arg1 = (char*)calloc((k+1),sizeof(char));
            memcpy(arg1, optarg, k++);
            double posx = atof(arg1);
            free(arg1);
            while(optarg[k] != '\0'){ k++; j++;}
            arg1 = (char*)calloc((j+1),sizeof(char));
            memcpy(arg1, optarg+k-j, j);
            double posy = atof(arg1);
            free(arg1);
            if(silent)printf("Sending desired movement vector...\n");
            msg[i++] = constructMessage(TBLRDR_MOVEBY, posx, posy);
            break;
        }
        case 'h':
        {
            int k=0,j=0;
            unsigned long ipnumber; unsigned short port;
            while(optarg[k] != ':' && optarg[k] != '\0'){ k++;}
            char* arg1 = (char*)calloc((k+1),sizeof(char));
            memcpy(arg1, optarg, k++);
            ipnumber = inet_addr(arg1);
            printf("%ld\n", ipnumber);
            for(int portC=0;portC<k;portC++) arg1[portC] = '\0';
            free(arg1);
            while(optarg[k] != '\0'){ k++; j++;}
            arg1 = (char*)calloc((j+1),sizeof(char));
            memcpy(arg1, optarg+k-j, j);
            printf("%s\n", arg1);
            port = (unsigned short) atoi(arg1);
            free(arg1);
            uint8_t* ip = (uint8_t*)&ipnumber;
            printf("Setting John to %d.%d.%d.%d:%d...\n", ip[0],ip[1], ip[2], ip[3], port);
            msg[i++] = constructMessage(TBLRDR_JOHN, ipnumber, port);
            break;
        }
        case 'i':
        {
            double pos= atof(optarg);
            if(silent)printf("Sending movement vector...\n");
            msg[i++] = constructMessage(TBLRDR_MOVEFWD, pos);
            break;
        }
        case 'j':
        {
            double angle = atof(optarg);
            printf("Sending command TurnTo %f...\n", angle);
            msg[i++] = constructMessage(TBLRDR_TURNTO, angle);
            break;
        }
        case 'k':
        {
            double angle = atof(optarg);
            printf("Sending command TurnBy %f...\n", angle);
            msg[i++] = constructMessage(TBLRDR_TURNBY, angle);
            break;
        }
        case 'l':
        {
            if(silent)printf("Requesting encoder data...\n");
            msg[i++] = constructMessage(TBLRDR_GETENC);
            break;
        }
        case 'm':{
            uint8_t arg = atoi(optarg);
            msg[i++] = constructMessage(TBLRDR_GETDIST, arg);
            break;
        }
        case 'n':{
            int k=0,j=0;
            unsigned long ipnumber; unsigned short port;
            while(optarg[k] != ':' && optarg[k] != '\0'){ k++;}
            char* arg1 = (char*)calloc((k+1),sizeof(char));
            memcpy(arg1, optarg, k++);
            ipnumber = inet_addr(arg1);
            free(arg1);
            while(optarg[k] != '\0'){ k++; j++;}
            arg1 = (char*)calloc((j+1),sizeof(char));
            memcpy(arg1, optarg+k-j, j);
            port = (unsigned short) atoi(arg1);
            free(arg1);
            msg[i++] = constructMessage(TBLRDR_REQPOS, ipnumber, port);
            break;
        }
        case 'v':{ silent=1; break;}

        case 'w':
        {
            printf("I will do next step maaaastah\n");
            msg[i++] = constructMessage(TBLRDR_NEXTSTEP);
            break;
        }

        case 'x':
        {
            printf("I will do all next step maaaastah\n");
            msg[i++] = constructMessage(TBLRDR_ALLSTEPS);
            break;
        }
        case 'y':
        {
            printf("I start maaaastah\n");
            msg[i++] = constructMessage(TBLRDR_START);
            break;
        }
        case 'z':
        {
            printf("I stop maaaastah\n");
            msg[i++] = constructMessage(TBLRDR_STOP);
            break;
        }
        case 'A':
        {
            printf("I update maaaastah");
            msg[i++] = constructMessage(TBLRDR_UPDATE);
            break;
        }
        case 'B':
        {
            printf("I get ze current maaaastah");
            msg[i++] = constructMessage(TBLRDR_GETCUR);
            break;
        }
        case 'C':
        {
            printf("I get ze reeeeal current maaaastah");
            msg[i++] = constructMessage(TBLRDR_GETRCR);
            break;
        }
        case 'D':
        {
            int k=0,j=0;
            while(optarg[k] != ',' && optarg[k] != '\0'){ k++;}
            char* arg1 = (char*)calloc((k+1),sizeof(char));
            memcpy(arg1, optarg, k++);
            long posx = (long)atof(arg1);
            for(int q=0;q<k-1;q++) arg1[q] = '\0';
            free(arg1);
            while(optarg[k] != '\0'){ k++; j++;}
            arg1 = (char*)calloc((j+1),sizeof(char));
            memcpy(arg1, optarg+k-j, j);
            long posy = (long)atof(arg1);
            free(arg1);
            //if(silent)printf("Sending desired movement vector...\n");
            msg[i++] = constructMessage(TBLRDR_CONTROLINIT, posx, posy);
            break;
        }
        case 'E':
        {
            printf("Debuggar ReglerDone");
            msg[i++] = constructMessage(TBLRDR_CONTROLDONE);
            break;
        }
        case 'F':
        {
            printf("Testing left motor...\n");
            msg[i++] = constructMessage(TBLRDR_TESTLMOTOR);
            break;
        }
        case 'G':
        {
            printf("Testing right motor...\n");
            msg[i++] = constructMessage(TBLRDR_TESTRMOTOR);
            break;
        }

        default:
        {
        }
        }
    }

    i=0;
    const char *str;
    while(msg[i] != NULL)
    {
        char* s = (char*)msg[i];
        //for(int j=0;j<18;j++) printf("%c ", s[j]);
        client.send_msg(msg[i], getsz(msg[i]));
        str = client.receive_msg();
        printf("%s\n", str);
        //unsigned short* t = (unsigned short*)msg[i];
     //   if(silent)printf("%d\t%d\n", t[0], t[1]);
        i++;
    }
    client.close_connection();
    return 0;
}

int getsz(void* msg)
{
    unsigned short* temp = (unsigned short*) msg;
    unsigned short t = temp[0];
    switch(t)
    {
    case TBLRDR_PING:
        return 2;
    case TBLRDR_GETPOS:
        return 2;
    case TBLRDR_SETPOS:
        return 18;
    case TBLRDR_GETANGLE:
        return 2;
    case TBLRDR_SETANGLE:
        return 10;
    case TBLRDR_MOVETO:
        return 18;
    case TBLRDR_MOVEBY:
        return 18;
    case TBLRDR_JOHN:
        return 8;
    case TBLRDR_MOVEFWD:
        return 10;
    case TBLRDR_TURNTO:
        return 10;
    case TBLRDR_TURNBY:
        return 10;
    case TBLRDR_GETENC:
        return 2;
    case TBLRDR_GETDIST:
        return 3;
    case TBLRDR_REQPOS:
        return 8;
    case TBLRDR_NEXTSTEP:
        return 2;
    case TBLRDR_ALLSTEPS:
        return 2;
    case TBLRDR_START:
        return 2;
    case TBLRDR_STOP:
        return 2;
    case TBLRDR_UPDATE:
        return 2;
    case TBLRDR_GETCUR:
        return 2;
    case TBLRDR_GETRCR:
        return 2;
    case TBLRDR_CONTROLINIT:
        return 10;
    case TBLRDR_CONTROLDONE:
        return 2;
    case TBLRDR_TESTLMOTOR:
        return 2;
    case TBLRDR_TESTRMOTOR:
        return 2;
    default:
        return 0;
    }
}

/*void sendMsg(ComClient client, void* msg)
{
    client.send_msg(msg, getsz(msg));
}*/

void* constructMessage(unsigned short Cmd, ...)
{
    unsigned char* temp = (unsigned char*)malloc(getsz(&Cmd));
    va_list args;
    va_start(args, Cmd);
    int x = sizeof(int);
    int y = sizeof(double);

    switch(Cmd)
    {
    case TBLRDR_PING:{
        break;
    }
    case TBLRDR_GETPOS:{
        break;
    }

    case TBLRDR_SETPOS:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, y);
        t = va_arg(args, double);
        memcpy(temp+6, &t, y);
        break;
    }
    case TBLRDR_GETANGLE:{
        break;
    }
    case TBLRDR_SETANGLE:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, y);
        break;
    }
    case TBLRDR_MOVETO:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, y);
        t = va_arg(args, double);
        memcpy(temp+10, &t, y);
        break;
    }
    case TBLRDR_MOVEBY:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, x);
        t = va_arg(args, double);
        memcpy(temp+10, &t, x);
        break;
    }
    case TBLRDR_JOHN:{
        unsigned long t = (unsigned long)va_arg(args, unsigned int);
        memcpy(temp+2, &t, x);
        unsigned short k = (unsigned short)va_arg(args, unsigned int);
        memcpy(temp+6, &k, 2);
        break;
    }
    case TBLRDR_MOVEFWD:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, y);
        break;
    }
    case TBLRDR_TURNTO:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, y);
        break;
    }
    case TBLRDR_TURNBY:{
        double t = va_arg(args, double);
        memcpy(temp+2, &t, y);
        break;
    }
    case TBLRDR_GETENC:{
        break;
    }
    case TBLRDR_GETDIST:{
        int t = va_arg(args, int);
        memcpy(temp+2, &t, 1);
        break;
    }
    case TBLRDR_REQPOS:{
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        unsigned short k = (unsigned short)va_arg(args, unsigned int);
        memcpy(temp+6, &k, 2);
        break;
    }
    case TBLRDR_NEXTSTEP:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_ALLSTEPS:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_START:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_STOP:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_GETCUR:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_GETRCR:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_CONTROLINIT:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        t = va_arg(args, int);
        memcpy(temp+6, &t, x);
        break;
    }
    case TBLRDR_CONTROLDONE:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_TESTLMOTOR:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    case TBLRDR_TESTRMOTOR:
    {
        int t = va_arg(args, int);
        memcpy(temp+2, &t, x);
        break;
    }
    default:{}
    }
    unsigned short* tempo = (unsigned short*)temp;
    //printf("We did construct a message.\n");
    tempo[0] = Cmd;
    int i;
    //for(i=0;i<10;i++)printf("%d ", (uint8_t)*(temp+i));
    va_end (args);
    return temp;
}


